
/*This code is used to plan the trajectory of the robot  
*/

#include <ros/ros.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "nav_msgs/Odometry.h"
#include "nav_msgs/Path.h"
#include "geometry_msgs/PoseStamped.h"
#include <tf/transform_listener.h>
#include <iostream>
#include <fstream>
#include <tinyxml.h>

#include "std_msgs/String.h"

using namespace std;
using namespace tf;

float x_current;
float y_current;

float normeNextGoal;

double last_angle=0;
class Waypoint_goal
{
public:
  struct Point
  {
    string name;
    float x;
    float y;

    float z;
    float w;
    bool visited;
  };

  vector<Point> Points;

  //Path_planned(float x, float y, bool visited);
  void addpointGoal(string Name, float X, float Y, float Z, float W, bool visit);
};

void Waypoint_goal::addpointGoal(string Name, float X, float Y, float Z, float W, bool visit) {
    Waypoint_goal::Point newPoint;
    newPoint.name = Name;
    newPoint.x = X;
    newPoint.y = Y;
    newPoint.z = Z;
    newPoint.w = W;
    newPoint.visited = visit;
    Points.push_back(newPoint);
}

Waypoint_goal newWaypoint;

// 加载导航点数据
bool LoadWaypointsFromFile(string inFilename)
{
    TiXmlDocument docLoad(inFilename);
    bool resLoad = docLoad.LoadFile();
    if(resLoad == false)
    {
        ROS_INFO("Failed to load waypoints... filename = %s", inFilename.c_str());
        return false;
    }

    TiXmlElement* RootElement = docLoad.RootElement();
    for(TiXmlNode* item = RootElement->FirstChild("Waypoint");item;item = item->NextSibling("Waypoint"))
    {
        TiXmlNode* child = item->FirstChild();
        const char* name = child->ToElement()->GetText();
        ROS_INFO("Load waypoint : %s", name);

        child = item->IterateChildren(child);
        const char* pos_x = child->ToElement()->GetText();

        child = item->IterateChildren(child);
        const char* pos_y = child->ToElement()->GetText();

        child = item->IterateChildren(child);
        const char* pos_z = child->ToElement()->GetText();

        child = item->IterateChildren(child);
        const char* ori_x = child->ToElement()->GetText();

        child = item->IterateChildren(child);
        const char* ori_y = child->ToElement()->GetText();

        child = item->IterateChildren(child);
        const char* ori_z = child->ToElement()->GetText();

        child = item->IterateChildren(child);
        const char* ori_w = child->ToElement()->GetText();

        newWaypoint.addpointGoal(  string(name), atof(pos_x),  atof(pos_y),  atof(ori_z),  atof(ori_w), false);
        cout <<"debug cwx:"<< string(name)  <<string(pos_x)  <<string(pos_y)  <<string(pos_z)  <<string(ori_z)  <<string(ori_w)  <<endl;
    }
}

class quaternion_ros
{
public:
  float w;
  float x;
  float y;
  float z;

  quaternion_ros();

  void toQuaternion(float pitch, float roll, float yaw);
};

void quaternion_ros::toQuaternion(float pitch, float roll, float yaw)
{

  float cy = cos(yaw * 0.5);
  float sy = sin(yaw * 0.5);
  float cr = cos(roll * 0.5);
  float sr = sin(roll * 0.5);
  float cp = cos(pitch * 0.5);
  float sp = sin(pitch * 0.5);

  w = cy * cr * cp + sy * sr * sp;
  x = cy * sr * cp - sy * cr * sp;
  y = cy * cr * sp + sy * sr * cp;
  z = sy * cr * cp - cy * sr * sp;
}

quaternion_ros::quaternion_ros()
{
  w = 1;
  x = 0;
  y = 0;
  z = 0;
}

class Path_planned
{
public:
  struct Goal
  {
    float x;
    float y;
    bool visited;
  };

  vector<Goal> Path;

  Path_planned();
  //Path_planned(float x, float y, bool visited);
  void addGoal(float X, float Y, bool visit);
};

Path_planned::Path_planned()
{
}

//Path_planned(float x, float y, bool visited)

void Path_planned::addGoal(float X, float Y, bool visit)
{
  Path_planned::Goal newGoal;
  newGoal.x = X;
  newGoal.y = Y;
  newGoal.visited = visit;
  Path.push_back(newGoal);
}

Path_planned planned_path;
nav_msgs::Path passed_path;
ros::Publisher pub_passed_path;

void pose_callback(const geometry_msgs::Pose &poses)
{ //里程计回调函数,用来计算当前机器人位置与前面目标点的距离,判断是否要发新的幕摆点
  x_current = poses.position.x;
  y_current = poses.position.y;
  passed_path.header.frame_id ="map";
  passed_path.header.stamp = ros::Time::now();
  geometry_msgs::PoseStamped p;
  p.header = passed_path.header;
  p.pose = poses;
  passed_path.poses.emplace_back(p);
  pub_passed_path.publish(passed_path);
}

int taille_last_path = 0;
bool new_path = false;

//接受规划的路径
void path_callback(const nav_msgs::Path &path)
{
  //注意为了rviz显示方便 路径一直在发,但是这里只用接受一次就好,当规划的路径发生变化时候再重新装载
  if ((planned_path.Path.size() == 0) || (path.poses.size() != taille_last_path))
  {
    planned_path.Path.clear();
    new_path = true;
    for (int i = 0; i < path.poses.size(); i++)
    {
      planned_path.addGoal(path.poses[i].pose.position.x, path.poses[i].pose.position.y, false);

      cout << path.poses[i].pose.position.x << " " << path.poses[i].pose.position.y << endl;
    }
    cout << "Recv path size:" << path.poses.size() << endl;
    taille_last_path = path.poses.size();
  }
}

// int **count_antonin(char *)

int main(int argc, char *argv[])
{
//   srand(time(0));
//   读取xml定点导航点数据
  ros::init(argc, argv, "next_goal");
  ros::NodeHandle next_goal;

  string waypoint_path;
  next_goal.param<string>("waypointXML",  waypoint_path, "/home/wheeltec-client/cwx_worksp/qt_rviz_hub2/src/Ros_Qt_Rviz_GUI-main/exist_waypoint/waypoint.xml");
  LoadWaypointsFromFile(waypoint_path);

  ros::Subscriber sub1 = next_goal.subscribe("/robot_pose_me", 10, pose_callback);
  ros::Subscriber sub2 = next_goal.subscribe("/path_planning_node/cleaning_plan_nodehandle/cleaning_path", 10, path_callback);

  ros::Publisher pub1 = next_goal.advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal", 10);
  pub_passed_path = next_goal.advertise<nav_msgs::Path>("/clean_robot/passed_path", 10);

  ros::Publisher finish_pub =  next_goal.advertise<std_msgs::String>("/clean_robot/finish", 1000);

  ros::Rate loop_rate(10);

  geometry_msgs::PoseStamped goal_msgs;
  int count = 0;
  int count_s = 0;
  bool full_finished = false;
  bool way_finished = false;
  double angle;
  bool goal_reached = false;
  bool goal_reached_s = false;
  //获取发送下一个点的阈值
  if (!next_goal.getParam("/NextGoal/tolerance_goal", normeNextGoal))
  {
    ROS_ERROR("Please set your tolerance_goal");
    return 0;
  }
  ROS_INFO("tolerance_goal=%f", normeNextGoal);

  while (ros::ok())
  {
    ros::spinOnce();
    if (new_path)
    {
      count = 0;
      new_path = false;
    }

    //行驶全路径规划算法路径
    if (!planned_path.Path.empty() || count+1 <planned_path.Path.size() )
    {
      //当前距离达到了
      if (sqrt(pow(x_current - planned_path.Path[count].x, 2) + pow(y_current - planned_path.Path[count].y, 2)) <= normeNextGoal)
      {
        if (count < planned_path.Path.size() - 1) {
          count++;
          goal_reached = false;
        } else if (count == planned_path.Path.size() - 1) {
          full_finished = true;
        }
      }
      if (goal_reached == false)
      {
        goal_msgs.header.frame_id = "map";
        goal_msgs.header.stamp = ros::Time::now();
        goal_msgs.pose.position.x = planned_path.Path[count].x;
        goal_msgs.pose.position.y = planned_path.Path[count].y;
        goal_msgs.pose.position.z = 0;
        if (count < planned_path.Path.size())
        {//计算发布的yaw，不过还有bug 但是不影响使用，yaw不会产生太大影响
          angle = atan2(planned_path.Path[count + 1].y - planned_path.Path[count].y, planned_path.Path[count + 1].x - planned_path.Path[count].x &&  count < planned_path.Path.size() - 1);
        }
        else
        {
          angle = atan2(planned_path.Path[0].y - planned_path.Path[count].y, planned_path.Path[0].x - planned_path.Path[count].x);
        }
      if (std::abs(last_angle - angle) < 0.17 &&  count>0 &&  count < planned_path.Path.size() - 1 ) {
          count++;
          continue;
      }

      last_angle = angle;

        quaternion_ros q;
        q.toQuaternion(0, 0, float(angle));
        goal_msgs.pose.orientation.w = q.w;
        goal_msgs.pose.orientation.x = q.x;
        goal_msgs.pose.orientation.y = q.y;
        goal_msgs.pose.orientation.z = q.z;

        cout << " NEW GOAL " << endl;
        cout << " x = " << planned_path.Path[count].x << " y = " << planned_path.Path[count].y << endl;

        goal_reached = true;
        pub1.publish(goal_msgs);
      }
    }

    // 全路径规划走完后,开始走算法不能规划到的定点路径
    if (full_finished && !newWaypoint.Points.empty()) {
        if (sqrt(pow(x_current - newWaypoint.Points[count_s].x, 2) + pow(y_current - newWaypoint.Points[count_s].y, 2)) <= 0.5)
        {
            // count_s++;
            // goal_reached_s = false;
            if (count_s < newWaypoint.Points.size() - 1) {
                count_s++;
                goal_reached_s = false;
            } else if (count_s == newWaypoint.Points.size() - 1) {
                way_finished = true;
                std_msgs::String output_msg;
                output_msg.data = "Route planning is complete";
                finish_pub.publish(output_msg);
                full_finished = true;
            }
        }
        if (goal_reached_s == false)
        {
            goal_msgs.header.frame_id = "map";
            goal_msgs.header.stamp = ros::Time::now();
            goal_msgs.pose.position.x = newWaypoint.Points[count_s].x;
            goal_msgs.pose.position.y = newWaypoint.Points[count_s].y;
            goal_msgs.pose.position.z = 0;
            goal_msgs.pose.orientation.w = newWaypoint.Points[count_s].w;
            goal_msgs.pose.orientation.x = 0;
            goal_msgs.pose.orientation.y = 0;
            goal_msgs.pose.orientation.z = newWaypoint.Points[count_s].z;

            cout << " NEW GOAL count:"<< count_s << "  x = " << newWaypoint.Points[count_s].x << "  y = " << newWaypoint.Points[count_s].y << endl;
            goal_reached_s = true;

            pub1.publish(goal_msgs);
        }
    }

    if (way_finished) {
        ROS_INFO("Break while!!!");
        return 0;
    }
    loop_rate.sleep();
  }

  return 0;
}
